
/*
 * @Date: 2020-11-27 11:45:09
 * @Description: ESP32 Camera Surveillance Car
 * @FilePath: 
 */

#include "esp_camera.h"
#include <WiFi.h>
#include <esp_now.h>

//
// WARNING!!! Make sure that you have either selected ESP32 Wrover Module,
//            or another board which has PSRAM enabled
//
// Adafruit ESP32 Feather

// Select camera model
//#define CAMERA_MODEL_WROVER_KIT
//#define CAMERA_MODEL_M5STACK_PSRAM
#define CAMERA_MODEL_AI_THINKER

const char* ssid = "chging";        //WIFI Name
const char* password = "1993@Chg";  //WIFI Password

#if defined(CAMERA_MODEL_WROVER_KIT)
#define PWDN_GPIO_NUM -1
#define RESET_GPIO_NUM -1
#define XCLK_GPIO_NUM 21
#define SIOD_GPIO_NUM 26
#define SIOC_GPIO_NUM 27

#define Y9_GPIO_NUM 35
#define Y8_GPIO_NUM 34
#define Y7_GPIO_NUM 39
#define Y6_GPIO_NUM 36
#define Y5_GPIO_NUM 19
#define Y4_GPIO_NUM 18
#define Y3_GPIO_NUM 5
#define Y2_GPIO_NUM 4
#define VSYNC_GPIO_NUM 25
#define HREF_GPIO_NUM 23
#define PCLK_GPIO_NUM 22


#elif defined(CAMERA_MODEL_AI_THINKER)
#define PWDN_GPIO_NUM 32
#define RESET_GPIO_NUM -1
#define XCLK_GPIO_NUM 0
#define SIOD_GPIO_NUM 26
#define SIOC_GPIO_NUM 27

#define Y9_GPIO_NUM 35
#define Y8_GPIO_NUM 34
#define Y7_GPIO_NUM 39
#define Y6_GPIO_NUM 36
#define Y5_GPIO_NUM 21
#define Y4_GPIO_NUM 19
#define Y3_GPIO_NUM 18
#define Y2_GPIO_NUM 5
#define VSYNC_GPIO_NUM 25
#define HREF_GPIO_NUM 23
#define PCLK_GPIO_NUM 22

#else
#error "Camera model not selected"
#endif

// GPIO Setting
extern int gpLb = 13;  // Left 1
extern int gpLf = 15;  // Left 2
extern int gpRb = 2;   // Right 1
extern int gpRf = 14;  // Right 2
extern int gpLed = 4;  // Light

extern String WiFiAddr = "";

void startCameraServer();

float car_speed = 0.67;  // m/s

typedef struct {
  uint8_t type;
  float distance;
  float time;
  uint8_t data[8];
} data_fmt_t;

// 新建队列句柄
QueueHandle_t dataQueue;

void setup() {
  Serial.begin(115200);
  Serial.setDebugOutput(true);
  Serial.println();


  pinMode(gpLb, OUTPUT);   //Left Backward
  pinMode(gpLf, OUTPUT);   //Left Forward
  pinMode(gpRb, OUTPUT);   //Right Forward
  pinMode(gpRf, OUTPUT);   //Right Backward
  pinMode(gpLed, OUTPUT);  //Light

  //initialize
  digitalWrite(gpLb, LOW);
  digitalWrite(gpLf, LOW);
  digitalWrite(gpRb, LOW);
  digitalWrite(gpRf, LOW);
  digitalWrite(gpLed, LOW);

  camera_config_t config;
  config.ledc_channel = LEDC_CHANNEL_0;
  config.ledc_timer = LEDC_TIMER_0;
  config.pin_d0 = Y2_GPIO_NUM;
  config.pin_d1 = Y3_GPIO_NUM;
  config.pin_d2 = Y4_GPIO_NUM;
  config.pin_d3 = Y5_GPIO_NUM;
  config.pin_d4 = Y6_GPIO_NUM;
  config.pin_d5 = Y7_GPIO_NUM;
  config.pin_d6 = Y8_GPIO_NUM;
  config.pin_d7 = Y9_GPIO_NUM;
  config.pin_xclk = XCLK_GPIO_NUM;
  config.pin_pclk = PCLK_GPIO_NUM;
  config.pin_vsync = VSYNC_GPIO_NUM;
  config.pin_href = HREF_GPIO_NUM;
  config.pin_sscb_sda = SIOD_GPIO_NUM;
  config.pin_sscb_scl = SIOC_GPIO_NUM;
  config.pin_pwdn = PWDN_GPIO_NUM;
  config.pin_reset = RESET_GPIO_NUM;
  config.xclk_freq_hz = 20000000;
  config.pixel_format = PIXFORMAT_JPEG;
  //init with high specs to pre-allocate larger buffers
  if (psramFound()) {
    config.frame_size = FRAMESIZE_UXGA;
    config.jpeg_quality = 10;
    config.fb_count = 2;
  } else {
    config.frame_size = FRAMESIZE_SVGA;
    config.jpeg_quality = 12;
    config.fb_count = 1;
  }

  // 初始化队列（队列长度可根据需求调整，这里设为10）
  dataQueue = xQueueCreate(10, sizeof(data_fmt_t));

  // 创建处理任务（优先级可根据需求调整，这里设为5）
  xTaskCreatePinnedToCore(
    dataProcessTask,
    "DataProcessTask",
    4096,
    NULL,
    5,
    NULL,
    APP_CPU_NUM);

  // camera init
  esp_err_t err = esp_camera_init(&config);
  if (err != ESP_OK) {
    Serial.printf("Camera init failed with error 0x%x", err);
    return;
  }

  //drop down frame size for higher initial frame rate
  sensor_t* s = esp_camera_sensor_get();
  s->set_framesize(s, FRAMESIZE_CIF);

  WiFi.mode(WIFI_AP_STA);

  WiFi.begin(ssid, password);

  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
    Serial.print(".");
  }
  Serial.println("");
  Serial.println("WiFi connected");

  startCameraServer();

  Serial.print("Camera Ready! Use 'http://");
  Serial.print(WiFi.localIP());
  WiFiAddr = WiFi.localIP().toString();
  Serial.println("' to connect");

  // WiFi.mode(WIFI_AP);                 //开启热点，AP模式
  WiFi.softAP("Slave_Car", NULL, 6);     // 关键：使用相同信道，热点名字，如果需要添加密码，就WiFi.softAP("Slave_1"，密码)
  esp_now_init();                        //ESP now初始化
  esp_now_register_recv_cb(OnDataRecv);  //用于接受数据
}

void loop() {

  // put your main code here, to run repeatedly:
}

//回调函数
void OnDataRecv(const uint8_t* mac_addr, const uint8_t* data, int data_len) {
  data_fmt_t recvData;
  memcpy(&recvData, data, sizeof(data_fmt_t));

  // 将数据推入队列
  if (xQueueSend(dataQueue, &recvData, pdMS_TO_TICKS(100)) != pdPASS) {
    Serial.println("Queue full, data discarded!");
  }
}

// 新建数据处理任务
void dataProcessTask(void* pvParameter) {
  data_fmt_t taskData;

  while (1) {
    // 从队列中取出数据（阻塞等待）
    if (xQueueReceive(dataQueue, &taskData, portMAX_DELAY)) {
      // 原有数据处理逻辑
      if (taskData.type == 0) {
        go_handler();
      } else if (taskData.type == 1) {
        back_handler();
      } else if (taskData.type == 2) {
        left_handler();
      } else if (taskData.type == 3) {
        right_handler();
      } else if (taskData.type == 4) {
        stop_handler();
      } else if (taskData.type == 5) {
        ledon_handler();
      } else if (taskData.type == 6) {
        ledoff_handler();
      } else if (taskData.type == 7) {
        go_distance_handler(taskData.distance);
      } else if (taskData.type == 8) {
        back_distance_handler(taskData.distance);
      } else if (taskData.type == 9) {
        left_angle_handler(taskData.time);
      } else if (taskData.type == 10) {
        right_angle_handler(taskData.time);
      } else {
        Serial.println("error data received!");
      }
    }
  }
}

extern void WheelAct(int nLf, int nLb, int nRf, int nRb);

static void go_handler(void) {
  WheelAct(LOW, HIGH, LOW, HIGH);
  Serial.println("Go");
}
static void back_handler(void) {
  WheelAct(HIGH, LOW, HIGH, LOW);
  Serial.println("Back");
}

static void left_handler(void) {
  WheelAct(HIGH, LOW, LOW, HIGH);
  Serial.println("Left");
}
static void right_handler(void) {
  WheelAct(LOW, HIGH, HIGH, LOW);
  Serial.println("Right");
}

static void stop_handler(void) {
  WheelAct(LOW, LOW, LOW, LOW);
  Serial.println("Stop");
}

static void ledon_handler(void) {
  digitalWrite(gpLed, HIGH);
  Serial.println("LED ON");
}
static void ledoff_handler(void) {
  digitalWrite(gpLed, LOW);
  Serial.println("LED OFF");
}

static void go_distance_handler(float d) {
  Serial.println("Go");
  WheelAct(LOW, HIGH, LOW, HIGH);

  float delay_s = d / car_speed;
  uint32_t delay_ms = delay_s * 1000;
  delay(delay_ms);

  WheelAct(LOW, LOW, LOW, LOW);
  Serial.print("Stop:");
  Serial.println(delay_ms);
}

static void back_distance_handler(float d) {
  Serial.println("Back");
  WheelAct(HIGH, LOW, HIGH, LOW);

  float delay_s = d / car_speed;
  uint32_t delay_ms = delay_s * 1000;
  delay(delay_ms);

  WheelAct(LOW, LOW, LOW, LOW);
  Serial.print("Stop:");
  Serial.println(delay_ms);
}

static void left_angle_handler(float t) {
  Serial.println("Left");
  WheelAct(HIGH, LOW, LOW, HIGH);

  float delay_s = t;
  uint32_t delay_ms = delay_s * 1000;
  delay(delay_ms);

  WheelAct(LOW, LOW, LOW, LOW);
  Serial.print("Stop:");
  Serial.println(delay_ms);
}

static void right_angle_handler(float t) {
  Serial.println("Right");
  WheelAct(LOW, HIGH, HIGH, LOW);

  float delay_s = t;
  uint32_t delay_ms = delay_s * 1000;
  delay(delay_ms);

  WheelAct(LOW, LOW, LOW, LOW);
  Serial.print("Stop:");
  Serial.println(delay_ms);
}